package com.hitqz.robot.driver.util;

import cn.hutool.core.collection.CollectionUtil;
import cn.hutool.core.date.DateUtil;
import cn.hutool.core.util.NumberUtil;
import com.example.tutorial.AddressBookProtos;
import com.hitqz.robot.api.common.entity.RobotMap;
import com.hitqz.robot.api.common.entity.RobotMapPos;
import com.hitqz.robot.driver.communication.NetworkServiceThreadPool;
import com.hitqz.robot.driver.dto.PowerInfoDto;
import com.hitqz.robot.driver.zmq.protocol.component.InDoorRobotComTool;
import com.hitqz.robot.driver.zmq.protocol.component.RouterThread;
import com.hitqz.robot.driver.zmq.protocol.component.WorkerThread;
import lombok.Synchronized;
import lombok.extern.slf4j.Slf4j;
import navconnector.NavconnectorBase;

import java.util.*;
import java.util.concurrent.*;
import java.util.concurrent.atomic.AtomicInteger;

@Slf4j
public class WheelToolkit {


    //获取消息结果
    public static final ConcurrentHashMap<String, CompletableFuture<Boolean>> requestMap = new ConcurrentHashMap<>();
    //机器人编码   要和导航那边配置文件对应
    public static String serialNumber = "";
    //zmq监听端口  要和导航那边配置文件对应
    private static final int defaultPort = 11003;
    //IPC协议地址 通常用于同一台机器上的不同进程之间的通信,其他ZeroMQ套接字可以通过该地址连接到这个套接字
    public static final String BACK_NAME = "hit-wheel-driver";
    //机器人地图
    public static RobotMap robotMap;
    //地图点位
    public static List<RobotMapPos> posList;
    //当前队列id，为空时才允许下发下个队列
    public static String currentTaskId = "";
    //队列
    public static Queue<Map<Integer, List<RobotMapPos>>> taskPosQueue = new LinkedBlockingDeque<>();

    private static final AtomicInteger seqSuffix = new AtomicInteger(0);

    //初始化通讯组件
    public static boolean init(Integer port) {
        int bindPort = port == null ? defaultPort : port;
        //路由线程
        NetworkServiceThreadPool.creatNetworkServerOrClient(new RouterThread(bindPort, BACK_NAME));
        //处理消息的线程
        for (int i = 0; i < 20; i++) {
            //fixme 先写死包名,后续修改
            NetworkServiceThreadPool.dealEvent(new WorkerThread(BACK_NAME,
                    "com.hitqz.robot.driver.zmq.protocol.component.ReceiveHandler"));
        }
        //发送消息队列的线程
        NetworkServiceThreadPool.creatNetworkServerOrClient(() -> {
            try {
                new InDoorRobotComTool(BACK_NAME);
            } catch (Exception e) {
                log.error("[wheel]init sendThread error", e);
            }
        });
        return true;
    }

    public static void dealTaskPosQueue(){
        if (WheelToolkit.taskPosQueue==null || WheelToolkit.taskPosQueue.isEmpty()){
            log.info("[wheel] taskPosQueue 为空");
            return;
        }
        //取出第一段队列(没有删除,到了站点才删除)
        Map<Integer, List<RobotMapPos>> posMap = taskPosQueue.peek();
        int navMode = 0;
        for (Integer key : posMap.keySet()) {
            navMode = key;
        }
        List<RobotMapPos> posList = posMap.get(navMode);
        //切换导航模式
        Integer currentNavMode = getNavMode();
        if (Objects.equals(navMode,currentNavMode)){
            setNavMode(navMode);
            log.info("[wheel] 切换导航模式:{}->{}",currentNavMode, navMode);
        }
        //切换地图
        String targetMapCode = posList.get(0).getMapCode();
        String currMapCode = getCurrMapCode();
        if (!Objects.equals(targetMapCode,currMapCode)){
            log.info("[wheel] 切换地图:{}->{}",currMapCode, targetMapCode);
            selMap(targetMapCode);
        }
        //下发队列
        setTaskQueue(posList);
        //启动队列
        cmdMotion(0);
    }

    /*****************************************************************************************************/

    /**
     * 设置定位开关
     *
     * @param cmd 0-关 1-开
     * @return true-设置成功 false-设置失败
     */
    public static boolean setLocationEnable(int cmd) {
        boolean result = false;
        int seq = generateSeq();
        try {
            InDoorRobotComTool.int32Command(WheelToolkit.serialNumber, seq, AddressBookProtos.Int32.OBJ_ID.CMD_LOC_MODE_REQ_VALUE, cmd);
            result = waitForResponse(seq, AddressBookProtos.Int32.OBJ_ID.CMD_LOC_MODE_REQ.name());
        } catch (Exception e) {
            log.error("[wheel]seq:{},{}定位失败,{}", seq, cmd == 0 ? "关闭" : "开启", e.getMessage());
        }
        return result;
    }

    /**
     * 获取定位开关状态
     *
     * @return 0-关 1-开
     */
    public static Integer getLocationEnableStatus() {
        int result = -1;
        int seq = generateSeq();
        try {
            InDoorRobotComTool.voidCommand(WheelToolkit.serialNumber, seq, AddressBookProtos.Void.OBJ_ID.GET_LOC_STATUS_REQ_VALUE);
            waitForResponse(seq, AddressBookProtos.Void.OBJ_ID.GET_LOC_STATUS_REQ.name());
        } catch (Exception e) {
            log.error("[wheel]seq:{},获取定位开关状态,{}", seq, e.getMessage());
        }
        return Optional.ofNullable((Integer) WheelCacheUtil.get(WheelCacheUtil.KEY_LOC_ENABLE_STATUS)).orElse(result);
    }

    /**
     * 设置定位模式
     *
     * @param mode 0-amcl 1-Cartographer 2-ndt
     * @return true-设置成功 false-设置失败
     */
    public static boolean setLocationMode(int mode) {
        boolean result = false;
        int seq = generateSeq();
        try {
            InDoorRobotComTool.int32Command(WheelToolkit.serialNumber, seq, AddressBookProtos.Int32.OBJ_ID.SEL_LOC_MODE_REQ_VALUE, mode);
            result = waitForResponse(seq, AddressBookProtos.Int32.OBJ_ID.SEL_LOC_MODE_REQ.name());
        } catch (Exception e) {
            log.error("[wheel]seq:{},设置定位模式{}失败,{}", seq, mode, e.getMessage());
        }
        return result;
    }


    /**
     * 获取当前定位模式
     *
     * @return 0-amcl 1-Cartographer 2-ndt
     */
    public static Integer getLocationMode() {
        Integer result = -1;
        int seq = generateSeq();
        try {
            InDoorRobotComTool.voidCommand(WheelToolkit.serialNumber, seq, AddressBookProtos.Void.OBJ_ID.GET_LOC_MODE_REQ_VALUE);
            waitForResponse(seq, AddressBookProtos.Void.OBJ_ID.GET_LOC_MODE_REQ.name());
        } catch (Exception e) {
            log.error("[wheel]seq:{},获取定位模式失败,{}", seq, e.getMessage());
        }
        return Optional.ofNullable((Integer) WheelCacheUtil.get(WheelCacheUtil.KEY_LOC_MODE)).orElse(result);
    }


    /**
     * 切换导航模式
     *
     * @param mode 0-空闲 1-2d建图  2-car..建图 3-导航 4-巡线 5-二维码 6-磁导航 7-3D建图
     *             8-Autoware 寻迹路径采集 9-循迹导航 10-规划导航 11-自动泊车 12-二次定位
     * @return true-设置成功 false-设置失败
     */
    public static boolean setNavMode(int mode) {
        boolean result = false;
        int seq = generateSeq();
        try {
            InDoorRobotComTool.int32Command(WheelToolkit.serialNumber, seq, AddressBookProtos.Int32.OBJ_ID.CMD_NAV_MODE_REQ_VALUE, mode);
            result = waitForResponse(seq, AddressBookProtos.Int32.OBJ_ID.CMD_NAV_MODE_REQ.name());
        } catch (Exception e) {
            log.error("[wheel]seq:{},设置导航模式 {} 失败,{}", seq, mode, e.getMessage());
        }
        return result;
    }

    /**
     * 获取导航状态
     *
     * @return 0-空闲 1-2d建图  2-car..建图 3-导航 4-巡线 5-二维码 6-磁导航 7-3D建图
     * 8-Autoware 寻迹路径采集 9-循迹导航 10-规划导航 11-自动泊车 12-二次定位
     */
    public static Integer getNavMode() {
        Integer result = -1;
        int seq = generateSeq();
        try {
            InDoorRobotComTool.voidCommand(WheelToolkit.serialNumber, seq, AddressBookProtos.Void.OBJ_ID.GET_NAV_MODE_REQ_VALUE);
            waitForResponse(seq, AddressBookProtos.Void.OBJ_ID.GET_NAV_MODE_REQ.name());
        } catch (Exception e) {
            log.error("[wheel]seq:{},获取导航模式失败,{}", seq, e.getMessage());
        }
        return Optional.ofNullable((Integer) WheelCacheUtil.get(WheelCacheUtil.KEY_NAV_MODE)).orElse(result);
    }

    /**
     * 设置初始位置
     *
     * @param x   x坐标
     * @param y   y坐标
     * @param yaw 方向角
     * @return true-设置成功 false-设置失败
     */
    public static boolean setInitPose(double x, double y, double yaw) {
        boolean result = false;
        int seq = generateSeq();
        try {
            NavconnectorBase.Point point = NavconnectorBase.Point.newBuilder().setX(x).setY(y).build();
            NavconnectorBase.Euler euler = NavconnectorBase.Euler.newBuilder().setYaw(yaw).build();
            NavconnectorBase.Pose pose = NavconnectorBase.Pose.newBuilder().setPosition(point).setOrientation(euler).build();
            InDoorRobotComTool.poseStampedCommand(WheelToolkit.serialNumber, seq,
                    AddressBookProtos.PoseStamped.OBJ_ID.SET_INIT_POSE_REQ_VALUE, pose);
            result = waitForResponse(seq, AddressBookProtos.PoseStamped.OBJ_ID.SET_INIT_POSE_REQ.name());
        } catch (Exception e) {
            log.error("[wheel]seq:{},设置初始位置 ({},{}) yaw:{} 失败,{}", seq, x, y, yaw, e.getMessage());
        }
        return result;
    }

    /**
     * 切换地图
     *
     * @param mapCode 地图编码-英文
     * @return true-设置成功 false-设置失败
     */
    public static boolean selMap(String mapCode) {
        boolean result = false;
        int seq = generateSeq();
        try {
            InDoorRobotComTool.stringCommand(WheelToolkit.serialNumber, seq, AddressBookProtos.String.OBJ_ID.SEL_MAP_REQ_VALUE, mapCode);
            result = waitForResponse(seq, AddressBookProtos.String.OBJ_ID.SEL_MAP_REQ.name());
        } catch (Exception e) {
            log.error("[wheel]seq:{},切换地图{}失败,{}", seq, mapCode, e.getMessage());
        }
        return result;
    }

    /**
     * 保存地图（建图流程）
     *
     * @param mapCode 地图编码
     * @return true-保存成功 false-保存失败
     */
    public static boolean saveMap(String mapCode) {
        boolean result = false;
        int seq = generateSeq();
        try {
            InDoorRobotComTool.stringCommand(WheelToolkit.serialNumber, seq, AddressBookProtos.String.OBJ_ID.SAV_MAP_REQ_VALUE, mapCode);
            result = waitForResponse(seq, AddressBookProtos.String.OBJ_ID.SAV_MAP_REQ.name());
        } catch (Exception e) {
            log.error("[wheel]seq:{},切换地图{}失败,{}", seq, mapCode, e.getMessage());
        }
        return result;
    }

    /**
     * 获取当前地图编码
     *
     * @return 地图编码
     */
    public static String getCurrMapCode() {
        String result = "";
        int seq = generateSeq();
        try {
            InDoorRobotComTool.voidCommand(WheelToolkit.serialNumber, seq, AddressBookProtos.Void.OBJ_ID.GET_CUR_MAP_NAME_REQ_VALUE);
            waitForResponse(seq, AddressBookProtos.Void.OBJ_ID.GET_CUR_MAP_NAME_REQ.name());
        } catch (Exception e) {
            log.error("[wheel]seq:{},获取当前地图编码失败,{}", seq, e.getMessage());
        }
        return Optional.ofNullable((String) WheelCacheUtil.get(WheelCacheUtil.KEY_CURR_MAP_NAME)).orElse(result);
    }

    /**
     * 获取导航地图编码列表
     *
     * @param type 2D 3D PB
     * @return 对应的地图编码列表
     */
    public static List<String> getMapCodes(String type) {
        List<String> result = new ArrayList<>(4);
        int seq = generateSeq();
        try {
            switch (type.toUpperCase()) {
                case "2D" -> {
                    InDoorRobotComTool.stringArrayCommand(WheelToolkit.serialNumber, seq, AddressBookProtos.StringArray.OBJ_ID.GET_MAP_2D_NAME_REQ_VALUE, result);
                    waitForResponse(seq, AddressBookProtos.StringArray.OBJ_ID.GET_MAP_2D_NAME_REQ.name());
                    result = Optional.ofNullable((List<String>) WheelCacheUtil.get(WheelCacheUtil.KEY_MAP_2D_NAMES)).orElse(result);
                }
                case "3D" -> {
                    InDoorRobotComTool.stringArrayCommand(WheelToolkit.serialNumber, seq, AddressBookProtos.StringArray.OBJ_ID.GET_MAP_3D_NAME_REQ_VALUE, result);
                    waitForResponse(seq, AddressBookProtos.StringArray.OBJ_ID.GET_MAP_3D_NAME_REQ.name());
                    result = Optional.ofNullable((List<String>) WheelCacheUtil.get(WheelCacheUtil.KEY_MAP_3D_NAMES)).orElse(result);
                }
                case "PB" -> {
                    InDoorRobotComTool.stringArrayCommand(WheelToolkit.serialNumber, seq, AddressBookProtos.StringArray.OBJ_ID.GET_MAP_PB_NAME_REQ_VALUE, result);
                    waitForResponse(seq, AddressBookProtos.StringArray.OBJ_ID.GET_MAP_PB_NAME_REQ.name());
                    result = Optional.ofNullable((List<String>) WheelCacheUtil.get(WheelCacheUtil.KEY_MAP_PB_NAMES)).orElse(result);
                }
            }
        } catch (Exception e) {
            log.error("[wheel]seq:{},获取{}地图编码列表失败,{}", seq, type, e.getMessage());
        }
        return result;
    }


    //todo 按类型下载地图
    public static RobotMap downloadMapFromNav(String type, String mapCode) {

        return null;
    }


    /**
     * 点云数据上报开关（前端展示地图上的激光轮廓）
     *
     * @param cmd 0-关闭 1-开启
     * @return true-设置成功 false-设置失败
     */
    public static boolean setPointsSend(int cmd) {
        boolean result = false;
        int seq = generateSeq();
        try {
            InDoorRobotComTool.int32Command(WheelToolkit.serialNumber, seq, AddressBookProtos.Int32.OBJ_ID.SET_POINTS_SEND_REQ_VALUE, cmd);
            result = waitForResponse(seq, AddressBookProtos.Int32.OBJ_ID.SET_POINTS_SEND_REQ.name());
        } catch (Exception e) {
            log.error("[wheel]seq:{},点云数据上报开关{} 失败,{}", seq, cmd == 0 ? "关闭" : "开启", e.getMessage());
        }
        return result;
    }

    /**
     * 设置速度
     *
     * @param linearX  x轴线速度
     * @param linearY  y轴线速度
     * @param linearZ  z轴线速度
     * @param angularX x轴角速度
     * @param angularY y轴角速度
     * @param angularZ z轴角速度
     */
    public static void setSpeed(double linearX, double linearY, double linearZ,
                         double angularX, double angularY, double angularZ) {
        boolean result = false;
        int seq = generateSeq();
        try {
            InDoorRobotComTool.twistStampedCommand(WheelToolkit.serialNumber, seq, AddressBookProtos.TwistStamped.OBJ_ID.SET_CMD_VEL_REQ_VALUE,
                    linearX, linearY, linearZ, angularX, angularY, angularZ);
        } catch (Exception e) {
            log.error("[wheel]seq:{},设置速度linear({},{},{}),angular({},{},{}) 失败,{}", seq, linearX, linearY, linearZ, angularX, angularY, angularZ, e.getMessage());
        }
    }

    /**
     * 设置巡线、二维码、磁导航队列
     * @param posList 要走的点位队列（同一个导航模式的）
     * @return true-设置成功 false-设置失败
     */
    public static boolean setTaskQueue(List<RobotMapPos> posList){
        boolean result = false;
        int seq = generateSeq();
        // 这里需要把点位队列转换成AddressBookProtos.Station
        List<AddressBookProtos.Station> list = new LinkedList<>();
        StringBuffer sb = new StringBuffer();
        for (RobotMapPos pos : CollectionUtil.emptyIfNull(posList)) {
            //巡线坐标为激光坐标；二维码坐标为二维码间的相对坐标，其中角度为弧度；
            NavconnectorBase.Euler euler = NavconnectorBase.Euler.newBuilder().setYaw(pos.getYaw()).build();
            NavconnectorBase.Point point = NavconnectorBase.Point.newBuilder().setX(pos.getPosX()).setY(pos.getPosY()).build();
            NavconnectorBase.Pose pose = NavconnectorBase.Pose.newBuilder().setPosition(point).setOrientation(euler).build();
            int type = pos.getQueueType();
            AddressBookProtos.Station station;
            if(Objects.equals(type, QueueTypeEnum.QRCODE.getType())) {
                station = AddressBookProtos.Station.newBuilder().setPose(pose).setIndex(pos.getId().intValue())
                        .setType(type).setLabel(Integer.parseInt(pos.getCode())).build();
            }else {
                station = AddressBookProtos.Station.newBuilder().setPose(pose).setIndex(pos.getId().intValue())
                        .setType(type).build();
            }
            list.add(station);
            // 日志打印路径
            sb.append(pos.getCode()).append("->");
        }
        //去掉最后一个->
        sb.delete(sb.length() - 2, sb.length());
        if (list.isEmpty()){
            log.warn("[wheel]队列为空,不下发");
            return false;
        }
        try {
            currentTaskId = "task"+seq;
            InDoorRobotComTool.stationArrayCommand(WheelToolkit.serialNumber, seq, currentTaskId , list);
            result = waitForResponse(seq,AddressBookProtos.StationArray.OBJ_ID.SET_STATION_TASK_REQ.name());
            log.error("[wheel]seq:{},设置队列{} 成功", seq,sb);
        } catch (Exception e) {
            log.error("[wheel]seq:{},设置队列{} 失败,{}", seq,sb,e.getMessage());
        }
        return result;
    }

    /**
     * 导航队列命令  （正常的流程：路径规划将路径点按导航模式拆分->设置队列->下发开始->导航上报完成->设置下一个队列->...->走完所有路径到目标点）
     * @param cmd 0 开始 1 停止 2 继续 3 暂停 （注意，如果暂停后如果有下发新的队列覆盖，再下发继续导航会报错）
     * @return true-下发成功 false-下发失败
     */
    public static boolean cmdMotion(int cmd){
        //避免 task变finish会触发发下一个队列
        if (cmd==1 || cmd == 3){
            taskPosQueue.clear();
            log.info("清空队列缓存");
        }
        boolean result = false;
        int seq = generateSeq();
        try {
            InDoorRobotComTool.int32Command(WheelToolkit.serialNumber, seq, AddressBookProtos.Int32.OBJ_ID.CMD_MOTION_REQ_VALUE, cmd);
            result = waitForResponse(seq,AddressBookProtos.Int32.OBJ_ID.CMD_MOTION_REQ.name());
        } catch (Exception e) {
            log.error("[wheel]seq:{},队列命令{} 执行失败,{}", seq,cmd, e.getMessage());
        }
        return result;
    }

    /**
     * 充电开关
     * @param cmd 0-停止充电 1-开始充电
     * @return true-设置成功 false-设置失败
     */
    public static boolean setCharge(int cmd){
        boolean result = false;
        int seq = generateSeq();
        try {
            InDoorRobotComTool.int32Command(WheelToolkit.serialNumber, seq, AddressBookProtos.Int32.OBJ_ID.CMD_RECHARGE_REQ_VALUE, cmd);
            result = waitForResponse(seq,AddressBookProtos.Int32.OBJ_ID.CMD_RECHARGE_REQ.name());
            log.info("[wheel]seq:{},下发 {} 命令执行成功", seq,cmd==0?"停止充电":"开始充电");
        } catch (Exception e) {
            log.error("[wheel]seq:{},设置充放电命令{} 执行失败,{}", seq,cmd, e.getMessage());
        }
        return result;
    }

    /**
     * 获取充电状态
     * @return 0正常速度控制/1启动回充/2未找到红外信号/3收到充电桩握手信号/4充电正常进行中/5充电过程中出现异常
     * 6电池电量充满，充电结束/7充电停止后退/8充电过程中出现拍硬急停或者碰撞开关/9充电极电压检测故障
     * 10充电过程调整的距离达到最大值/11充电失败处理中/12充电失败处理完成/13上位机取消充电
     */
    public static int getChargeStatus(){
        int result = -1;
        int seq = generateSeq();
        try {
            InDoorRobotComTool.voidCommand(WheelToolkit.serialNumber, seq, AddressBookProtos.Void.OBJ_ID.GET_RECHARGE_STATUS_REQ_VALUE);
            waitForResponse(seq,AddressBookProtos.Void.OBJ_ID.GET_RECHARGE_STATUS_REQ.name());
            result =Optional.ofNullable((Integer) (WheelCacheUtil.get(WheelCacheUtil.KEY_RECHARGE_STATUS))).orElse(result);
        } catch (Exception e) {
            log.error("[wheel]seq:{},获取充电状态失败,{}", seq, e.getMessage());
        }
        return result;
    }

    /**
     * 获取电源信息
     * @return 电源信息
     */
    public static PowerInfoDto getPowerInfo(){
        PowerInfoDto powerInfoDto = new PowerInfoDto();
        int seq = generateSeq();
        try {
            //主动查询充电状态
            InDoorRobotComTool.voidCommand(WheelToolkit.serialNumber, seq, AddressBookProtos.Void.OBJ_ID.GET_RECHARGE_STATUS_REQ_VALUE);
            waitForResponse(seq,AddressBookProtos.Void.OBJ_ID.GET_RECHARGE_STATUS_REQ.name());
            //查询电源信息
            InDoorRobotComTool.voidCommand(WheelToolkit.serialNumber, seq, AddressBookProtos.Void.OBJ_ID.GET_POWER_INFO_REQ_VALUE);
            waitForResponse(seq,AddressBookProtos.Void.OBJ_ID.GET_POWER_INFO_REQ.name());
            powerInfoDto = Optional.ofNullable((PowerInfoDto) WheelCacheUtil.get(WheelCacheUtil.KEY_POWER_INFO)).orElse(powerInfoDto);
        } catch (Exception e) {
            log.error("[wheel]seq:{},获取电源信息失败,{}", seq, e.getMessage());
        }
        return powerInfoDto;
    }



    /*****************************************************************************************************/

    @Synchronized
    private static int generateSeq() {
        int suffix = seqSuffix.getAndIncrement();
        if (suffix>9){
            seqSuffix.set(0);
        }
        String timeStamp = DateUtil.format(new Date(), "HHmmssSSS")+suffix;
        return NumberUtil.parseInt(timeStamp);
    }


    /**
     * 加锁
     *
     * @param seq  消息序号 key
     * @param type 消息类型 如CMD_LOC_MODE_REQ,仅用于失败打印
     */
    public static boolean waitForResponse(Integer seq, String type) {
        CompletableFuture<Boolean> future = new CompletableFuture<>();
        requestMap.put(seq.toString(), future);
        try {
            return future.get(5, TimeUnit.SECONDS);
        } catch (InterruptedException | ExecutionException e) {
            throw new RuntimeException(e);
        } catch (TimeoutException e) {
            log.warn("[wheel]messag({}), {} send timeout",seq, type);
        }
        return false;
    }

    /**
     * 释放锁
     *
     * @param seq   消息序号 key
     * @param type  返回消息实体类名称 如AirQuality,仅用于失败打印
     * @param objId 返回消息实体类switch分支,仅用于失败打印
     */
    public static void release(Integer seq, String type, Integer objId, boolean result) {
        CompletableFuture<Boolean> future = requestMap.remove(seq.toString());
        if (future != null) {
            future.complete(result);
            log.debug("[wheel]message:{}.{},release", type, objId);
        }
    }

}
